#include <ros/ros.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <unistd.h>
#include <std_msgs/Float32MultiArray.h>

#include "tail_control.h"
#include "tail_motor_data.h"
#include "motor_ctrl.h"
#include "config.h"

extern ros::Publisher motor_pub[TAIL_MOTOR_NUM];

TailMotorData motor_data_ori[TAIL_MOTOR_NUM];       //电机原始数据
TailMotorData joint_data[TAIL_MOTOR_NUM];           //关节运动学数据
uint8_t vesc_round = 0;
extern bool start_flag;
Motor_Mode cur_mode[TAIL_MOTOR_NUM] = {MOTOR_MODE_EXIT, MOTOR_MODE_EXIT, MOTOR_MODE_EXIT};

int TailControl::excelDataRead()
{
    std::ifstream file(this->filename);
    if(!file.is_open()) {
        ROS_ERROR("Failed to open excel file\n");
        return -1;
    }
    
    std::string line;
    while(std::getline(file, line)) {
        std::stringstream ss(line);
        std::string cell;
        std::getline(ss, cell, ',');
        joint_feedforward_tau[0].push_back(std::stod(cell));
        std::getline(ss, cell, ',');
        joint_feedforward_tau[1].push_back(std::stod(cell));
        std::getline(ss, cell, ',');
        joint_position[0].push_back(std::stod(cell));
        std::getline(ss, cell, ',');
        joint_velocity[0].push_back(std::stod(cell));
        std::getline(ss, cell, ',');
        joint_position[1].push_back(std::stod(cell));
        std::getline(ss, cell, ',');
        joint_velocity[1].push_back(std::stod(cell));
    }

    std::cout << "joint0_feedforward_tau" <<std::endl;
    for(size_t i=0; i<joint_feedforward_tau[0].size(); i++) {
        std::cout << joint_feedforward_tau[0][i] << " ";
    }
    std::cout<<std::endl;
    std::cout << "joint1_feedforward_tau" <<std::endl;
    for(size_t i=0; i<joint_feedforward_tau[1].size(); i++) {
        std::cout << joint_feedforward_tau[1][i] << " ";
    }
    std::cout<<std::endl;

    file.close();
    return 0;
}


int TailControl::zeroPosSet()
{
    return -1;
}


int TailControl::comInit(Com_Desc &com_param)
{
    if(com_param.type == COM_USBCAN) {
        this->p_com_ctrl    = new USBCAN_Ctrl(com_param.param, com_param.len);
    } else {
        return -1;
    }

    this->p_com_ctrl->com_ctrl_init();
    return 0;
}

int TailControl::motorInit(Motor_Desc &motor_param)
{
    this->p_motor_ctrl = new Motor_Ctrl(p_com_ctrl, motor_param.motor_n, motor_param.motor_array);
    this->p_com_ctrl->com_thread_enable();
    return 0;
}

void pdController(double current_position, double current_velocity, double target_position, double target_velocity, double& command) 
{
    const double Kp = 1.0; 
    const double Kd = 0.1;
    double position_error = target_position - current_position;
    double velocity_error = target_velocity - current_velocity;
    command = Kp * position_error + Kd * velocity_error;
}


void TailControl::calculateTailPos(double& joint_command, uint8_t num)
{
    #define DIR   (-1)         //1为顺时针，-1为逆时针
    static double w = 1440.0f;                       //角速度
    static double rotate_delta = DIR*4*360.0f;      //待转角度
    static double pos_delta = DIR*(w*1.0f/hz);
    static double target_pos = \
            rotate_delta + ((Motor_VESC *)p_motor_ctrl->get_motor_item(num))->get_start_offset();
    double next_pos = joint_command*180.0f/3.14f + pos_delta;
    if(abs(next_pos) > abs(target_pos)) {
        return;
    }
    joint_command = next_pos/180.0f*3.14;
}

void TailControl::calculatePosCommand(double& joint_command, uint8_t num)
{
    //会保持在最后时刻位置
    joint_command = 0;
    static size_t cnt[TAIL_MOTOR_NUM] = {0};
    
    joint_command = joint_position[num][cnt[num]];
    if(cnt[num] < (joint_position[num].size()-1)) {
        ++cnt[num];
        if(cnt[num] == joint_position[num].size()) {
            start_flag = false;
        }
    } else {
        cur_mode[num] =  MOTOR_MODE_POS_HOLD;
    }
}

void TailControl::calculateTauCommand(double& joint_command, uint8_t num)
{
    //会保持在最后时刻位置
    joint_command = 0;
    static size_t cnt[TAIL_MOTOR_NUM] = {0};
    if(cnt[num] < joint_feedforward_tau[num].size()) {
        double feed_forward_torque_joint = joint_feedforward_tau[num][cnt[num]]; 

        double target_position_joint = joint_position[num][cnt[num]]; 
        double target_velocity_joint = joint_velocity[num][cnt[num]]; 


        pdController(joint_data[num].pos, joint_data[num].vel, target_position_joint, target_velocity_joint, joint_command);

        joint_command += feed_forward_torque_joint;

        ++cnt[num];
        if(cnt[num] == joint_feedforward_tau[num].size()) {
            start_flag = false;
        }
    } else {
        cur_mode[num]  =  MOTOR_MODE_POS_HOLD;
    }
}

void TailControl::posLimit()
{
    
}

extern std::shared_ptr<TailControl> control;
void TailControl::read()
{
    static bool motor3_flag = false;
    std_msgs::Float32MultiArray msg[TAIL_MOTOR_NUM];
    for(uint8_t i=0;i<TAIL_MOTOR_NUM;i++){
        pthread_mutex_lock(&control->mutex_lock);
        motor_data_ori[i].pos = control->p_motor_ctrl->get_motor_item(i)->get_cur_pos()/180.0f*3.14f;
        motor_data_ori[i].vel = control->p_motor_ctrl->get_motor_item(i)->get_cur_w()/180.0f*3.14f;
        motor_data_ori[i].tau = control->p_motor_ctrl->get_motor_item(i)->get_cur_tau();
        pthread_mutex_unlock(&control->mutex_lock);
        if((cur_mode[i] == MOTOR_MODE_T) && (motor_data_ori[i].tau_cmd < 0)) {
            motor_data_ori[i].tau = -motor_data_ori[i].tau;
        }
        if((!motor3_flag) && (i == (TAIL_MOTOR_NUM-1))) {   //尾翼电机起始偏移
            motor3_flag = true;
            ((Motor_VESC *)control->p_motor_ctrl->get_motor_item(i))->set_start_offset(motor_data_ori[i].pos*180.0f/3.14f);
            std::cout << "offset=" << ((Motor_VESC *)control->p_motor_ctrl->get_motor_item(i))->get_start_offset() << std::endl;
        }
        joint_data[i].pos = direction_motor[i]*motor_data_ori[i].pos + offset_motor[i];
        joint_data[i].vel = direction_motor[i]*motor_data_ori[i].vel;
        joint_data[i].tau = direction_motor[i]*motor_data_ori[i].tau;
        msg[i].data.push_back(i);
        msg[i].data.push_back(motor_data_ori[i].pos*180.0f/3.14f);
        msg[i].data.push_back(motor_data_ori[i].vel*180.0f/3.14f);
        msg[i].data.push_back(motor_data_ori[i].tau);
        motor_pub[i].publish(msg[i]);

    }

}

void TailControl::update()
{
    static bool  hold_flag[TAIL_MOTOR_NUM] = {false,false,false};       //在MOTOR_MODE_POS_HOLD模式时，防止电机读回值与设定值误差
    for(uint8_t i=0;i<TAIL_MOTOR_NUM;i++) {
        if((cur_mode[i] == MOTOR_MODE_ORIGIN) || (cur_mode[i] == MOTOR_MODE_EXIT) || (cur_mode[i] == MOTOR_MODE_ENTER)) {
            hold_flag[i] = false;;
        } else if(cur_mode[i] == MOTOR_MODE_POS_HOLD){
            if(!hold_flag[i]) {
                joint_data[i].pos_cmd = joint_data[i].pos;
                hold_flag[i] = true;
            }

        } else if(cur_mode[i] == MOTOR_MODE_POS_TRACK){
            if(i != (TAIL_MOTOR_NUM-1)) {
                calculatePosCommand(joint_data[i].pos_cmd, i);
            } else {
                calculateTailPos(joint_data[i].pos_cmd, i);
            }
            hold_flag[i] = false;;
        } else if(cur_mode[i] == MOTOR_MODE_T){
            #if 0
                calculateTauCommand(joint_data[i].tau_cmd, i);
            #else
                joint_data[i].tau_cmd = -0.3;
            #endif
            hold_flag[i] = false;; 
        } else {
            ;
        }
    }
}

void TailControl::write()
{
    double value[TAIL_MOTOR_NUM][3];
    double *pvalue[TAIL_MOTOR_NUM];
    uint8_t no[TAIL_MOTOR_NUM] = {0};
    uint8_t len[TAIL_MOTOR_NUM] = {0};
    posLimit();
    for (uint8_t i = 0; i < TAIL_MOTOR_NUM; ++i) {  
        motor_data_ori[i].pos_cmd = direction_motor[i]*(joint_data[i].pos_cmd - offset_motor[i]);
        motor_data_ori[i].tau_cmd = direction_motor[i]*joint_data[i].tau_cmd;
        motor_data_ori[i].vel_cmd = direction_motor[i]*joint_data[i].vel_cmd;
    } 

    for (uint8_t index = 0; index < TAIL_MOTOR_NUM; ++index) {  
        if((cur_mode[index] == MOTOR_MODE_POS_HOLD) ||(cur_mode[index] == MOTOR_MODE_POS_TRACK)) {
            value[index][0] = motor_data_ori[index].pos_cmd*180.0f/3.14f;
        } else if(cur_mode[index] == MOTOR_MODE_T){
            value[index][0] = motor_data_ori[index].tau_cmd;
        } else if(cur_mode[index] == MOTOR_MODE_ORIGIN){
            value[index][0] = 0;
        } else {       
            continue;
        }
        no[index] = index;
        len[index] = 1;
        pvalue[index] = value[index];
    }
    p_motor_ctrl->motor_func(TAIL_MOTOR_NUM, no, cur_mode, pvalue, len);
}


void TailControl::tailLoop()
{
    read();
    update();
    write();
}

void TailControl::tailInit()
{
    int ret = -1;

    pthread_mutex_init(&mutex_lock,NULL);
    //读取excel数据
    ret = this->excelDataRead();
    if(ret<0) {
        ROS_ERROR("excelDataRead err\n");
        return ;
    }

    //通信初始化
    Com_Desc com_param = {
        .len = 1,
        .param = {USBCAN_BAUD_1M},
        .type = COM_USBCAN,
    };
    ret = this->comInit(com_param);
    if(ret<0) {
        ROS_ERROR("comInit err\n");
        return ;
    }
    sleep(1);

    //电机初始化
    Motor_Desc motor_param = {
        .motor_n = 3,
        .motor_array = {
            {MOTOR_TYPE_AK, 1, 0, 0},
            {MOTOR_TYPE_AK, 2, 0, 0},
            {MOTOR_TYPE_VESC, 3, 0, 0},
        },
    };
    ret = this->motorInit(motor_param);
    if(ret<0) {
        ROS_ERROR("motorInit err\n");
        return ;
    }

    //创建线程,下发命令
    // start_flag = true;
    loop_thread = std::thread([&]() {
        bool zero_flag = false;
        extern bool start_flag;
        //校零
        sleep(5);   //此时间间隙，用手将尾翼拖放至零点位置
        std::cout << "start!" << std::endl;
        while (1) {
            if (start_flag) {
                for(uint8_t i=0;i<TAIL_MOTOR_NUM;i++) {
                    #if 1
                        cur_mode[i] = MOTOR_MODE_T;             //扭矩跟踪
                    #else 
                        cur_mode[i] = MOTOR_MODE_POS_TRACK;     //位置跟踪
                    #endif
                    if(i == (TAIL_MOTOR_NUM-1)) {
                        cur_mode[i] = MOTOR_MODE_POS_TRACK;    
                    }
                }
            } else {
                if(!zero_flag) {
                    for(uint8_t i=0;i<TAIL_MOTOR_NUM;i++) {
                        cur_mode[i] = MOTOR_MODE_ORIGIN;        //设置零点
                    }
                    zero_flag = true;
                } else {
                    #if 1
                        for(uint8_t i=0;i<TAIL_MOTOR_NUM;i++) {
                            cur_mode[i] = MOTOR_MODE_POS_HOLD;      //位置保持
                        }
                    #else
                        while(1) {sleep(1);}
                    #endif
                }
            }
            tailLoop();
            usleep(1000*1000/hz);
        }
    });

}


